#include "utils_fifo.h"
#include <string.h>
#include <stdio.h>
#include "hal_tty.h"

#define USE_FREERTOS		1

#if USE_FREERTOS
#include "FreeRTOS.h"
#include "task.h"

#define malloc  pvPortMalloc
#define free    vPortFree
#endif 

#define min(a, b) (((a) < (b)) ? (a) : (b))
#define is_power_of_2(x) ((x) != 0 && (((x) & ((x) - 1)) == 0))

static uint16_t _power_of_two(uint16_t _integer) {
    _integer |= (_integer >> 1);
    _integer |= (_integer >> 2);
    _integer |= (_integer >> 4);
    _integer |= (_integer >> 8);
    return (_integer + 1);
}

ring_buffer_t * ring_buffer_create(uint16_t size) {
    
	ring_buffer_t *r;
	
	if(!is_power_of_2(size)) {
        size = _power_of_two(size);
	}
	
	r = (ring_buffer_t *)pvPortMalloc(sizeof(ring_buffer_t));
	if(r == NULL) {
		goto malloc_ring_buffer_err;
	}

	r->buffer = (uint8_t *)malloc(size);
	if(r->buffer == NULL) {
		goto malloc_buffer_err;
	}
	
	r->size = size;
	r->_in = 0;
	r->_out = 0;
	
	return r;
	
malloc_buffer_err:	
	free(r);
malloc_ring_buffer_err:
	//@TODO through malloc ERR
	return NULL;
}

void ring_buffer_clr(ring_buffer_t *r) {
    r->_in = 0;
    r->_out = 0;
}

uint16_t ring_buffer_len(ring_buffer_t *r) {

#if USE_FREERTOS
	vTaskSuspendAll();
#endif	
	
	uint16_t size = (r->_in - r->_out);
	
#if USE_FREERTOS	
	xTaskResumeAll();
#endif
	
    return size;
}

uint16_t ring_buffer_put(ring_buffer_t *r,uint8_t *buf,uint16_t size) {
    uint16_t len = 0;
	if(buf == NULL || r == NULL) {
		return 0;
	}
	
#if USE_FREERTOS
	vTaskSuspendAll();
#endif	
	
    size = min(size, (uint16_t)(r->size - (r->_in - r->_out)));
    /* first put the data starting from fifo->in to buffer end */
	len  = min(size, r->size - (r->_in & (r->size - 1)));

	memcpy(r->buffer + (r->_in & (r->size - 1)), buf, len);
	/* then put the rest (if any) at the beginning of the buffer */
	memcpy(r->buffer, buf + len, size - len);
	r->_in += size;

#if USE_FREERTOS	
	xTaskResumeAll();
#endif
	
	return size;
}

uint16_t ring_buffer_get(ring_buffer_t *r,uint8_t *buf,uint16_t size) {
    
    uint16_t len = 0;
	if(buf == NULL || r == NULL)
		return 0;
	
#if USE_FREERTOS
	vTaskSuspendAll();
#endif	
	
	size  = min(size,(uint16_t)(r->_in - r->_out));
	/* first get the data from fifo->out until the end of the buffer */
	len = min(size, r->size - (r->_out & (r->size - 1)));
	memcpy(buf, r->buffer + (r->_out & (r->size - 1)), len);
	/* then get the rest (if any) from the beginning of the buffer */
	memcpy(buf + len, r->buffer, size - len);
	r->_out += size;
	
#if USE_FREERTOS	
	xTaskResumeAll();
#endif
	
	return size;
}

void ring_buffer_destory(ring_buffer_t *r) {
	if(r) {
		if(r->buffer) {
			free(r->buffer);
			r->buffer = NULL; 
		}
		free(r);
	}
}



